20. Particle Weights Solution

We can calculate particle weights using the following equation:

P(x,y) =\frac{1}{2\pi \sigma_{x}\sigma_{y}}e^{-(\frac{(x-\mu_x)^2}{2\sigma_x^2}+\frac{(y-\mu_y)^2}{2\sigma_y^2})}
  • Recall that for this example the standard deviation for both x and y is 0.3.
  • x and y are the observations in map coordinates from landmarks quiz and \mu_x , \mu_y are the coordinates of the nearest landmarks. These should correspond to the correct responses from previous quizzes.

Quiz Solutions

OBS1 weight should be around 0.00683644777551 rounding to 6.84E-3 .

OBS2 weight should be around 0.00683644777551 , rounding to 6.84E-3 .

OBS3 weight should be around 9.83184874151e-49 , rounding to 9.83E-49 .

The final weight should be around 4.59112934464959e-53 , rounding to 4.60E-53 (the above three multiplied together).

We have included a C++ solution below; the multiv_gauss.cpp file contains the primary calculations.

Start Quiz: 

#include <iostream>
#include "multiv_gauss.h"

int main() {
  // define inputs
  double sig_x, sig_y, x_obs, y_obs, mu_x, mu_y;
  // define outputs for observations
  double weight1, weight2, weight3;
  // final weight
  double final_weight;
    
  // OBS1 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 6;
  y_obs = 3;
  mu_x = 5;
  mu_y = 3;
  // Calculate OBS1 weight
  weight1 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 0.00683644777551 rounding to 6.84E-3
  std::cout << "Weight1: " << weight1 << std::endl;
    
  // OBS2 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 2;
  y_obs = 2;
  mu_x = 2;
  mu_y = 1;
  // Calculate OBS2 weight
  weight2 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 0.00683644777551 rounding to 6.84E-3
  std::cout << "Weight2: " << weight2 << std::endl;
    
  // OBS3 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 0;
  y_obs = 5;
  mu_x = 2;
  mu_y = 1;
  // Calculate OBS3 weight
  weight3 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 9.83184874151e-49 rounding to 9.83E-49
  std::cout << "Weight3: " << weight3 << std::endl;
    
  // Output final weight
  final_weight = weight1 * weight2 * weight3;
  // 4.60E-53
  std::cout << "Final weight: " << final_weight << std::endl;
    
  return 0;
}
#include "multiv_gauss.h"
#include <cmath>

double multiv_prob(double sig_x, double sig_y, double x_obs, double y_obs,
                   double mu_x, double mu_y) {
  // calculate normalization term
  double gauss_norm;
  gauss_norm = 1 / (2 * M_PI * sig_x * sig_y);

  // calculate exponent
  double exponent;
  exponent = (pow(x_obs - mu_x, 2) / (2 * pow(sig_x, 2)))
               + (pow(y_obs - mu_y, 2) / (2 * pow(sig_y, 2)));
    
  // calculate weight using normalization terms and exponent
  double weight;
  weight = gauss_norm * exp(-exponent);
    
  return weight;
}
#ifndef MULTIV_GAUSS_H
#define MULTIV_GAUSS_H

double multiv_prob(double sig_x, double sig_y, double x_obs, double y_obs,
                   double mu_x, double mu_y);

#endif  // MULTIV_GAUSS_H

Next Concept